#include <Arduino.h>
#include <ESP32Servo.h>
#define PWMA_PIN 16
#define AIN1_PIN 19
#define AIN2_PIN 18
#define PWMB_PIN 17
#define BIN1_PIN 26
#define BIN2_PIN 27
#define duoji_PIN 15
#define leftA_track_PIN 22
#define leftB_track_PIN 21
#define middle_track_PIN 5
#define righA_track_PIN 4
#define righB_track_PIN 23
Servo myservo;

float Kp = 13, Ki = 0, Kd = 1;                   //pid弯道参数参数
float error = 0, P = 0, I = 0, D = 0, PID_value = 0;//pid直道参数
//float decide = 0;                                   //元素判断
float previous_error = 0, previous_I = 0;           //误差值
int sensor[5] = {0, 0, 0, 0, 0};                    //5个传感器数值的数组
static int initial_duoji_angle = 85 ;               //舵机初始角度//需要改
volatile int flag = 0;                                       //停车标识
void setup() {
  myservo.attach(duoji_PIN, 1000, 2000);
  //    attachInterrupt(digitalPinToInterrupt(interruptPin), blink, FALLING);
  track_pinint();        //循迹引脚初始化
  motor_pinint();        //电机引脚初始化
  duoji_pinint();        //舵机引脚初始化
}

void loop() {
  // put your main code here, to run repeatedly:
  //    digitalWrite(interruptPin,LOW);
  read_sensor_stop();
  read_sensor_values();
  calc_pid();
  duoji_control();
  if (error > 0)
  {
    motorRun_turnright();
  }
  else if (error < 0)
  {
    motorRun_turnleft();
  }
  else
  {
    motorRun_forward();
  }
  delay(10);
}

/*循迹模块引脚初始化*/
void track_pinint()
{
  pinMode (leftA_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (leftB_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (middle_track_PIN, INPUT);//设置引脚为输入引脚
  pinMode (righA_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (righB_track_PIN, INPUT); //设置引脚为输入引脚
}

/*电机引脚初始化*/
void motor_pinint( )
{
  pinMode(PWMA_PIN, OUTPUT);
  pinMode(AIN1_PIN, OUTPUT);
  pinMode(AIN2_PIN, OUTPUT);

  pinMode(PWMB_PIN, OUTPUT);
  pinMode(BIN1_PIN, OUTPUT);
  pinMode(BIN2_PIN, OUTPUT);
}

/*循迹模块引脚初始化*/
void duoji_pinint( )
{
  pinMode(duoji_PIN, OUTPUT);
}

void read_sensor_stop()
{
  sensor[0] = digitalRead(leftA_track_PIN);
  sensor[1] = digitalRead(leftB_track_PIN);
  sensor[2] = digitalRead(middle_track_PIN);
  sensor[3] = digitalRead(righA_track_PIN);
  sensor[4] = digitalRead(righB_track_PIN);
  if ((sensor[1] == 1) && (sensor[3] == 1))
  {
    flag += 1;
    delay(50);
    if (flag > 20)
    {
      delay(150);
      motorStop();
    }
  }
}

void read_sensor_values()//循迹
{
  sensor[0] = digitalRead(leftA_track_PIN);
  sensor[1] = digitalRead(leftB_track_PIN);
  sensor[2] = digitalRead(middle_track_PIN);
  sensor[3] = digitalRead(righA_track_PIN);
  sensor[4] = digitalRead(righB_track_PIN);

  if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0)
      && (sensor[3] == 0) && (sensor[4] == 1))
  {
    error = 2;//          0 0 0 0 1
  }
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0)
           && (sensor[3] == 1) && (sensor[4] == 1))
  {
    error = 1.5;//      0 0 0 1 1
  }
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0)
           && (sensor[3] == 1) && (sensor[4] == 0))
  {
    error = 1;//          0 0 0 1 0
  }
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1)
           && (sensor[3] == 1) && (sensor[4] == 0))
  {
    error = 0.5;//      0 0 1 1 0
  }
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1)
           && (sensor[3] == 0) && (sensor[4] == 0))
  {
    error = 0;//           0 0 1 0 0
  }
  else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1)
           && (sensor[3] == 0) && (sensor[4] == 0))
  {
    error = -0.5;//       0 1 1 0 0
  }
  else if ((sensor[0] == 0 ) && (sensor[1] == 1) && (sensor[2] == 0)
           && (sensor[3] == 0) && (sensor[4] == 0))
  {
    error = -1;//         0 1 0 0 0
  }
  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0)
           && (sensor[3] == 0) && (sensor[4] == 0))
  {
    error = -1.5;//      1 1 0 0 0
  }
  else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0)
           && (sensor[3] == 0) && (sensor[4] == 0))
  {
    error = -2;//         1 0 0 0 0
  }
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0)
           && (sensor[3] == 0) && (sensor[4] == 0))
  {
    if (error == -2)
    { //  0 0 0 0 0
      error = -3;
    }
    else if (error == 2)
    {
      error = 3;
    }
  }
}

void calc_pid()
{
  P = error;
  I = I + error;
  D = error - previous_error;
  PID_value = (Kp * P) + (Ki * I) + (Kd * D);
  previous_error = error;
}

void duoji_control()//舵机控制
{
  int duoji_angle = initial_duoji_angle + PID_value;

  if (duoji_angle < 45)
  {
    duoji_angle = 45;
  }
  if (duoji_angle > 125)
  {
    duoji_angle = 125;
  }
  myservo.write(duoji_angle);
}


void motorRun_forward()//电机驱动//需要改
{
  digitalWrite(AIN1_PIN, LOW);
  digitalWrite(AIN2_PIN, HIGH);
  analogWrite(PWMA_PIN, 100);


  digitalWrite(BIN1_PIN, LOW);
  digitalWrite(BIN2_PIN, HIGH);
  analogWrite(PWMB_PIN, 95);
}

void motorRun_turnright()//电机驱动
{
  digitalWrite(AIN1_PIN, LOW);
  digitalWrite(AIN2_PIN, HIGH);
  analogWrite(PWMA_PIN, 120);


  digitalWrite(BIN1_PIN, LOW);
  digitalWrite(BIN2_PIN, HIGH);
  analogWrite(PWMB_PIN, 50);
}

void motorRun_turnleft()//电机驱动
{
  digitalWrite(AIN1_PIN, LOW);
  digitalWrite(AIN2_PIN, HIGH);
  analogWrite(PWMA_PIN, 50);
  digitalWrite(BIN1_PIN, LOW);
  digitalWrite(BIN2_PIN, HIGH);
  analogWrite(PWMB_PIN, 120);
}

void motorStop()//电机驱动
{
  digitalWrite(AIN1_PIN, LOW);
  digitalWrite(AIN2_PIN, LOW);
  digitalWrite(BIN1_PIN, LOW);
  digitalWrite(BIN2_PIN, LOW);
  analogWrite(PWMB_PIN, 0);
  myservo.write(85);
  delay(4000000);
}
